%% DrawDisp
figure(1);
for i = 1:PlanNum
    subplot(3,1,i);
    plot(lcgc(:,1),P(i,:))
end
figure(2);
for i = 1:PlanNum
    subplot(3,1,i);
    plot(lcgc(:,1),T(i,:))
end
figure(3);
plot(lcgc(:,1),Z_s(1,:),lcgc(:,1),Z_s(2,:),lcgc(:,1),Z_s(3,:));
legend('Z1','Z2','Z3');
title('Z')